#include <iostream>
#include <DeltaKinematics.h>
#include <QDebug>
#include <DeltaKinematicsKe.h>
#include "ur_kin.h"
#include "genhexkins.h"
using namespace std;

void testHexpod()
{
    HexPodKinematics hexKin;

    EmcPose  pos;

    pos.tran.x=10;
    pos.tran.y=85;
    pos.tran.z=350;
    pos.a=40;
    pos.b=10;
    pos.c=-40;

    qDebug()<<"initial pose:"<<pos.tran.x
                    <<pos.tran.y<<pos.tran.z<<pos.a<<pos.b<<pos.c;

    double joints[6];
    KINEMATICS_INVERSE_FLAGS  iflags;
    KINEMATICS_FORWARD_FLAGS  fflags;
    int tmpKey=hexKin.kinematicsInverse(&pos,joints,&iflags,&fflags);

    //1026.06,978.692,1039.101,1027.342,1015.565, 1076.142
    qDebug()<<"  kinematicsInverse result   "<<tmpKey<<"length:"<<joints[0]
                    <<joints[1]<<joints[2]<<joints[3]<<joints[4]<<joints[5];

    pos.tran.x+=8;
    pos.tran.y+=38;
    pos.tran.z+=-90;
//    pos.a+=5;
//    pos.b+=9;
//    pos.c+=-10;
    tmpKey=hexKin.kinematicsForward(joints,&pos,&iflags,&fflags);

    qDebug()<<"  kinematicsForward result   "<<tmpKey<<"coordinate:"<<pos.tran.x
                    <<pos.tran.y<<pos.tran.z<<pos.a<<pos.b<<pos.c;

}

typedef DeltaKinematics<double>::DeltaVector DV;


void testUr()
{
    double d1 =  0.1215;
    double a2 = -0.3;
    double a3 = -0.276;
    double d4 =  0.1105;
    double d5 =  0.09;
    double d6 =  0.082;
    UrKinematics urKin(d1,a2,a3,d4,d5,d6);
    //forward
    double qInit[6];
//    qInit[0]=0;
//    qInit[1]=0;
//    qInit[2]=0;
//    qInit[3]=0;
//    qInit[4]=0;
//    qInit[5]=0;

    qInit[0]=0;
    qInit[1]=-67/180.0*M_PI-M_PI_2;
//    qInit[1]+=2*M_PI;
    qInit[2]=88/180.0*M_PI;
    qInit[3]=35/180.0*M_PI-M_PI_2;
//    qInit[3]+=2*M_PI;
    qInit[4]=0;
    qInit[5]=0;

    double tOut[16];

    urKin.forward(qInit,tOut);

    qDebug()<<"  qinit "<<qInit[0]<<qInit[1]<<qInit[2]<<qInit[3]<<qInit[4]<<qInit[5];


    for(int i=0; i< 3;++i)
    {
//          p_out.M.data[i*3+0] = tOut[i*4+0];
//          p_out.M.data[i*3+1] = tOut[i*4+1];
//          p_out.M.data[i*3+2] = tOut[i*4+2];
//          p_out.p[i] = tOut[i*4+3];
          qDebug()<<"  rot  "<<tOut[i*4+0]<<tOut[i*4+1]<<tOut[i*4+2]<<"  pose "<<tOut[i*4+3];
     }

    //inverse
    double qOut[8*6];
    int solveNum;

    solveNum = urKin.inverse(tOut, qOut);
    for(int i=0;i<solveNum;i++)
    {
        qDebug()<<i<<"  qOut"<<qOut[0+i*6]<<qOut[1+i*6]
               <<qOut[2+i*6]<<qOut[3+i*6]<<qOut[4+i*6]<<qOut[5+i*6];

    }

}

// robot geometry
 // (look at pics above for explanation)
// const float e = 206.11;     // end effector
// const float f = 467.65;     // base
// const float re = 700;
// const float rf = 303.065;

// // trigonometric constants
// const float sqrt3 = sqrt(3.0);
// const float pi = 3.141592653;    // PI
// const float sin120 = sqrt3/2.0;
// const float cos120 = -0.5;
// const float tan60 = sqrt3;
// const float sin30 = 0.5;
// const float tan30 = 1/sqrt3;


//// forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
//// returned status: 0=OK, -1=non-existing position
//int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
//    float t = (f-e)*tan30/2;
//    float dtr = pi/(float)180.0;

//    theta1 *= dtr;
//    theta2 *= dtr;
//    theta3 *= dtr;

//    float y1 = -(t + rf*cos(theta1));
//    float z1 = -rf*sin(theta1);

//    float y2 = (t + rf*cos(theta2))*sin30;
//    float x2 = y2*tan60;
//    float z2 = -rf*sin(theta2);

//    float y3 = (t + rf*cos(theta3))*sin30;
//    float x3 = -y3*tan60;
//    float z3 = -rf*sin(theta3);

//    float dnm = (y2-y1)*x3-(y3-y1)*x2;

//    float w1 = y1*y1 + z1*z1;
//    float w2 = x2*x2 + y2*y2 + z2*z2;
//    float w3 = x3*x3 + y3*y3 + z3*z3;

//    // x = (a1*z + b1)/dnm
//    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
//    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

//    // y = (a2*z + b2)/dnm;
//    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
//    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

//    // a*z^2 + b*z + c = 0
//    float a = a1*a1 + a2*a2 + dnm*dnm;
//    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
//    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

//    // discriminant
//    float d = b*b - (float)4.0*a*c;
//    if (d < 0) return -1; // non-existing point

//    z0 = -(float)0.5*(b+sqrt(d))/a;
//    x0 = (a1*z0 + b1)/dnm;
//    y0 = (a2*z0 + b2)/dnm;
//    return 0;
//}

//// inverse kinematics
//// helper functions, calculates angle theta1 (for YZ-pane)
//int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
//    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
//    y0 -= 0.5 * 0.57735    * e;    // shift center to edge
//    // z = a + b*y
//    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
//    float b = (y1-y0)/z0;
//    // discriminant
//    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
//    if (d < 0) return -1; // non-existing point
//    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
//    float zj = a + b*yj;
//    theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
//    return 0;
//}

//// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
//// returned status: 0=OK, -1=non-existing position
//int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
//    theta1 = theta2 = theta3 = 0;
//    int status = delta_calcAngleYZ(x0, y0, z0, theta1);
//    if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2);  // rotate coords to +120 deg
//    if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
//    return status;
//}

int main()
{

//    float a,b,c,x,y,z;
//        //    a=15/180.0*M_PI;
//        //    b=15/180.0*M_PI;
//        //    c=15/180.0*M_PI;

////            a=10;
////            b=20;
////            c=30;

////            a=0;
////            b=0;
////            c=0;

//            double bias=8.156;
//            a=10+bias;//33.633
//            b=20+bias;//20.264
//            c=30+bias;//18.247
//            delta_calcForward(a,b,c,x,y,z);
//            qDebug()<<"forward　"
//                                  <<"result 坐标"<<x<<y<<z;//0位置时，z=638

    testHexpod();
//    testUr();

//if(0)
//{
//    //    DeltaKinematicsKe test_robot(0.303,0.7,0.135,0.0595);
//    //    DeltaKinematicsKe test_robot(0.30,0.043,0.7,0.135,0.0595,0);
//        DeltaKinematicsKe test_robot(300,43,700,135,59.5,0);
//        double a,b,c,x,y,z;
//    //    a=15/180.0*M_PI;
//    //    b=15/180.0*M_PI;
//    //    c=15/180.0*M_PI;

//        a=6.095;
//        b=44.441;
//        c=50.176;

////        a=0;
////        b=0;
////        c=0;

//    //    double bias=8.156;
//    //    a=20.264+bias;//33.633
//    //    b=33.633+bias;//20.264
//    //    c=18.247+bias;//18.247

//        if(1==test_robot.forward_bias(a,b,c,x,y,z))
//        {
//                qDebug()<<"forward　"
//                      <<"result 坐标"<<x<<y<<z;//0位置时，z=638
//        }
//        else
//        {
//            qDebug()<<"forward error, no result";
//        }
//    //    x=0;
//    //    y=0;
//    //    z=-0.606;

//        if(1==test_robot.inverse_bias(x,y,z,a,b,c))
//        {
//                qDebug()<<"inverse　"
//                      <<"result 关节"<<a<<b<<c;
//        }
//        else
//        {
//            qDebug()<<" inverse error, no result";
//        }
//}
//else
//{

//    //    DeltaKinematics<double>::DeltaGeometricDim test_robot_dim = {sb,sp,L,l,0.070,-35,55};
//    //    DeltaKinematics<double> test_robot(test_robot_dim);
//    //    DeltaKinematics<double>::DeltaGeometricDim test_robot_dim = {660,90,200,530,70,-5,55};
//    //    deltaRobotKinetic=new DeltaKinematics<double>((*robotAttributeIn)[0].dhParameter[3],(*robotAttributeIn)[0].dhParameter[0],
//    //            (*robotAttributeIn)[0].dhParameter[1],(*robotAttributeIn)[0].dhParameter[2],(*robotAttributeIn)[1].dhParameter[0],
//    //            (*robotAttributeIn)[1].dhParameter[3],(*robotAttributeIn)[1].dhParameter[1],(*robotAttributeIn)[1].dhParameter[2]);
//        DeltaKinematics<double> test_robot(0.135,0.3,0.043,0.7,0.0595,0,-30,35.0);//3,0,1,2 ,0,3,1,2
//    //    DeltaKinematics<double> test_robot(300,43,700,135,59.5,-35,55,0);

//    //    DV my_vector = {0,0,-0.627,0,0,0};
//        DV my_vector = {0,0,0,0,0,0};
//    //    my_vector.Print();

//    //    test_robot.CalculateIpk_bias(&my_vector, 1);
//    //    qDebug()<<"1CalculateIpk 坐标"<<my_vector.x<<my_vector.y<<my_vector.z
//    //           <<"result 关节"<<my_vector.phi1<<my_vector.phi2<<my_vector.phi3;
//    ////    my_vector.Print();
//    //    my_vector.z = 0.0;
//        qDebug()<<"1CalculateFpk　关节"<<my_vector.phi1
//               <<my_vector.phi2<<my_vector.phi3;
//        test_robot.CalculateFpk_bias(&my_vector, 1);
//    //    my_vector.Print();
//        qDebug()<<"1CalculateFpk　关节"<<my_vector.phi1
//               <<my_vector.phi2<<my_vector.phi3
//              <<"result 坐标"<<my_vector.x<<my_vector.y<<my_vector.z;
//}

//    while (1) {

//    }
    return 0;
}
